#include "WPILib.h"
#include "Robot2489.h"



void Robot2489::LineFollow()
{
	bool left; 
	bool mid; 
	bool right;
	
	//timer->Start();
	deployer->Set(0.0);

	while (!(left && mid && right) && IsAutonomous() ) //when the sensor is over a line, the output is 0, ie FALSE
	{
		left = !leftLineSensor->Get(); //CHANGED LOGIC. left = 1 implies ON line, etc.
		mid = !middleLineSensor->Get();
		right = !rightLineSensor->Get();
		onMidInSwitch = !midInSwitch->Get();
		
		screen->PrintfLine(DriverStationLCD::kUser_Line1, "left = %d", left);
		screen->PrintfLine(DriverStationLCD::kUser_Line2, "mid = %d", mid);
		screen->PrintfLine(DriverStationLCD::kUser_Line3, "right = %d", right);
		screen->UpdateLCD();
		/*
		if (midInSwitch->Get() && !timer->HasPeriodPassed(2.0))
		{
			ManipUp();
		}
		else
		{
			ManipStop();
		}*/
		if (mid && !right && !left)
		{
			myRobot->MecanumDrive_Cartesian(0, 0.4, 0);
		}
		else if ((left || (left && mid)) && !right)
		{
			myRobot->MecanumDrive_Cartesian(0.3, 0.3, 0.05);
		}
		else if ((right || (right && mid)) && !left)
		{
			myRobot->MecanumDrive_Cartesian(-0.3, 0.3, -0.05);
		}
	}
	myRobot->MecanumDrive_Cartesian(0, 0, 0);
	//ManipStop();
}

void Robot2489::LineV() //in the first instance when all light sensors are activiated, the robot is on the V
{
	bool left;
	bool mid;
	bool right;

	while (left && right && mid)
	{
		left = !leftLineSensor->Get();
		mid = !middleLineSensor->Get();
		right = !rightLineSensor->Get();

		screen->PrintfLine(DriverStationLCD::kUser_Line1, "left = %d", left);
		screen->PrintfLine(DriverStationLCD::kUser_Line2, "mid = %d", mid);
		screen->PrintfLine(DriverStationLCD::kUser_Line3, "right = %d", right);
		screen->UpdateLCD();
		
		myRobot->MecanumDrive_Cartesian(-0.15, 0.15, -0.05);
	}
	myRobot->MecanumDrive_Cartesian(0, 0, 0);
}
